#include <Eigen/Sparse>
#include<Eigen/Dense>
#include <iostream>
#include <chrono>
#include <fstream>
#include "engineer_minimum_snap.hpp"
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;

using namespace hitcrt::engineer_spline;
void load_eigenMatrix(Eigen::MatrixXd &joint_matrix){
    std::ifstream in_file("/home/hitcrt/engineer_arm/joint_matrix.bin", std::ios::binary);
    if (!in_file) {
        std::cerr << "无法打开文件读取！" << std::endl;
        return ;
    }

    // 读取矩阵维度
    int rows, cols;
    in_file.read(reinterpret_cast<char*>(&rows), sizeof(rows));
    in_file.read(reinterpret_cast<char*>(&cols), sizeof(cols));

    // 创建矩阵并读取数据
    joint_matrix = Eigen::MatrixXd(rows, cols);
    in_file.read(reinterpret_cast<char*>(joint_matrix.data()), 
               rows * cols * sizeof(double));
    
    in_file.close();

    // // 验证读取结果
    // std::cout << "读取的矩阵 (" 
    //           << rows << "x" << cols << "):\n" 
    //           << joint_matrix << std::endl;
    return ;
}


void plot3SmoothedTrajectory(
    const Eigen::MatrixXd & joint_matrix,
    const std::vector<minimum_snap>& joint_splines_pos,
    const Eigen::VectorXd& times)
{
    const int num_points = joint_matrix.cols();
    const int num_joints = 7;
    const int plot_samples = 100;

    std::vector<std::vector<double>> original_joints(num_joints);
    std::vector<double> times_vec(times.data(), times.data() + times.size());

    for (int j = 0; j < num_joints; ++j) {
        original_joints[j].resize(num_points);
        for (int i = 0; i < num_points; ++i) {
            original_joints[j][i] = joint_matrix(j,i);
            // std::cout<<"要画的散点图的("<<j<<","<<i<<")"<<original_joints[j][i]<<std::endl;
        }
    }

    auto plot_derivative = [&](int order, const std::string& title_suffix, const std::string& ylabel) {
        plt::figure_size(1000, 1600);
        // plt::title(title_suffix);
        for (int j = 0; j < num_joints; ++j) {//7个轴挨个画
            plt::subplot(7, 1, j + 1);
            if(order==0){
                plt::scatter(times_vec, original_joints[j], 15, 
                            {{"color", "red"}, {"label", "Control Points"}});//画角度的时候把原来的也画上去
            }
            std::vector<double> curve_t, curve_y;
            curve_t.reserve(plot_samples);
            curve_y.reserve(plot_samples);

            for (int k = 0; k < plot_samples; ++k) {
                double h = 1e-5;
                double t = static_cast<double>(k) / (plot_samples - 1);//自变量因为时间单位是归一化的，所以这个是这样子的
                double y = 0.0;


                auto something = joint_splines_pos[j];
                y = something.f_t(t);


                // double t = static_cast<double>(k) / (plot_samples - 1);
                // double y = joint_splines_pos[j].derivative(order)(t)[0];
                curve_t.push_back(t);
                curve_y.push_back(y);
            }

            plt::plot(curve_t, curve_y, {{"label", title_suffix + " Spline"}, {"color", "blue"}});
            plt::title("Joint " + std::to_string(j + 1) + " - " + title_suffix);
            plt::xlabel("Normalized Time");
            plt::ylabel(ylabel);
            plt::grid(true);
            plt::legend();
        }

        plt::tight_layout();
        plt::show(false);
    };

    // 位置曲线
    plot_derivative(0, "Position", "Angle (rad)");
    // // 速度曲线
    // plot_derivative(1, "Velocity", "Velocity (rad/s)");
    // // 加速度曲线
    // plot_derivative(2, "Acceleration", "Acceleration (rad/s^2)");
    plt::show();

    std::cout << "全部绘图完成" << std::endl;
}

int test_plot() {
    Eigen::MatrixXd joint_matrix(7,10);
    const int num_points = joint_matrix.cols();
    joint_matrix << 
    0,  10,  20,  30,  40,  50,  60,  70,  80,  90,
    0,   5,  10,  15,  20,  25,  30,  35,  40,  45,
    0,  10,  20,  30,  40,  50,  60,  70,  80,  90,
    0,   8,  16,  24,  32,  40,  48,  56,  64,  72,
    0,   5,  10,  15,  20,  25,  30,  35,  40,  45,
    0,   9,  18,  27,  36,  45,  54,  63,  72,  81,
    0,  15,  30,  45,  60,  75,  90, 105, 120, 135;
    
    // Eigen::MatrixXd temp_load;
    // load_eigenMatrix(temp_load);
    // joint_matrix = temp_load.topRows(7);
    // std::cout<<"一共有"<<    joint_matrix.cols()<<std::endl;

    // 创建参数化时间向量 (基于累积关节变化量)
    // 假设角速度是一定的 ，这个假设还可以改进
    Eigen::VectorXd times(num_points);//数据总数，保证时间是统一的
    // times = temp_load.row(7); 
    double t_total = 3.0;
    double t_sum = 0.0;
    for(int i=0;i<num_points-1;i++){
        t_sum += std::abs(joint_matrix(0,i+1)-joint_matrix(0,i));
        t_sum += std::abs(joint_matrix(1,i+1)-joint_matrix(1,i));
        t_sum += std::abs(joint_matrix(2,i+1)-joint_matrix(2,i));
        t_sum += std::abs(joint_matrix(3,i+1)-joint_matrix(3,i));
        t_sum += std::abs(joint_matrix(4,i+1)-joint_matrix(4,i));
        t_sum += std::abs(joint_matrix(5,i+1)-joint_matrix(5,i));
        t_sum += std::abs(joint_matrix(6,i+1)-joint_matrix(6,i));//使用1范数
        t_sum = std::max(0.01,t_sum);//一次至少变化这么多
        times(i+1) = t_sum;
    }
    // times(0) = times(1)/2;

    for(int i = 0;i<num_points;i++){
        times(i) = times(i)/t_sum*t_total;
    }
    std::cout<<"看看这个时间\n"<<times<<std::endl;
    std::vector<minimum_snap> spline_vec;
    for(int i=0;i<7;i++){
        Eigen::VectorXd positions = Eigen::Map<Eigen::VectorXd>(
            joint_matrix.data() + i*joint_matrix.cols(), 
            joint_matrix.cols()
        );
        // std::cout<<"第"<<i<<"个positions\n"<<positions<<std::endl;
        minimum_snap spline_temp;
        spline_temp.generate_spline(times,positions,50,20,5);
        spline_vec.push_back(spline_temp);
    }
    plot3SmoothedTrajectory(joint_matrix,spline_vec,times);
    // const Eigen::RowVectorXd new_times = Eigen::RowVectorXd::LinSpaced(joint_matrix.cols()*1.5, 0.0, 1.0);
    // for (int i = 0; i < joint_matrix.cols()*1.5; ++i) {
    //     std::vector<double> joint_angles(7);
        
    //     for (int j = 0; j < 7; ++j) {
    //         // 评估样条曲线获取关节角度
    //         joint_angles[j] = joint_splines_pos[j](new_times[i])[0];
    //     }
        
    //     smoothed_trajectory.push_back(joint_angles);
    // }
    
    return 0;
}

int main(){
    test_plot();
        // EigenVEC_pair minimum_snap::generate_d(
        // const Eigen::VectorXd &positions, const int &n_order,
        // const Eigen::VectorXd &begin_constraint,
        // const Eigen::VectorXd &end_constraint,const Eigen::VectorXd& limits) 
//     minimum_snap instance;
//     Eigen::VectorXd positions(5);
//     positions<<1,2,3,4,5;

//     int n_order = 5;
//     Eigen::VectorXd begin_constraint(6);
//     begin_constraint<<1,0,0,0,0,0;
//     Eigen::VectorXd end_constraint(6);
//     end_constraint<<5,0,0,0,0,0;
//     Eigen::VectorXd limits(5);
//     limits<<50,50,INFINITY,INFINITY,INFINITY;
//     auto [low,up] = instance.generate_d(positions,n_order,begin_constraint,end_constraint,limits);
// std::cout<<"low\n"<<low<<std::endl;
// std::cout<<"测试up\n"<<up<<std::endl;

    // auto mat = instance.generate_A(n_order,times);
    // std::cout<<"测试\n"<<Eigen::MatrixXd(mat)<<std::endl;
    return 0;
}